"""
Create: JiBingyu 2024-07-25
Description: 和main.py一样的逻辑，但是主要用来分析平台环境
Update:
1.速度的设定没有限制，但是随着输入速度增大（1000左右），实际速度会有一定的下降
2.每次控制仿真的执行时间大概为0.1s，因此每次运动距离为0.1*速度
3.Yaw的最大变化角度是1度
4.Vel的变化倍数是输入变化的0.017986297607421875倍（当前0，输入100，实际变为1.7986145）
"""

import ctypes
import math
import os
import shutil
import threading
import time
from typing import List

import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np

from Function_j import Go_to_xy
from MultiUSVModel import MultiUSVModel
from StructType import *


# 清空dataRecord文件夹
def clear_dataRecord_folder():
    folder_path = "dataRecord"  # 指定 dataRecord 文件夹的路径
    for filename in os.listdir(folder_path):
        file_path = os.path.join(folder_path, filename)
        try:
            if os.path.isfile(file_path) or os.path.islink(file_path):
                os.unlink(file_path)
            elif os.path.isdir(file_path):
                shutil.rmtree(file_path)
        except Exception as e:
            print(f"Failed to delete {file_path}. Reason: {e}")


clear_dataRecord_folder()

# 全局变量定义
Task_data = []  # 任务点， taskPath_list并不是每次都有，所以需要一个全局变量来存储
Refe_data = []  # 参考点， referInfo_list并不是每次都有，所以需要一个全局变量来存储
Obs_data = []  # 障碍物，其中obsR和fOBSV与说明书上相反，代码中半径为0速度为200，说明书上半径为200速度为0
USV_data = []  # 实时的USV位置信息
plotting = True  # 是否绘图
# 各艇控制指令
multiUSVInitInfo: List[USVInitStructType] = []
Flag_once = True

Old_data = np.zeros((6, 4))


def algorithmImpl(
    algIterNum,
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    global Flag_once
    global Old_data
    multiUSVStatus_list = Get_USV_data(
        taskPath_data,
        taskPath_size,
        referInfo_data,
        referInfo_size,
        curOBSFusion_data,
        curOBSFusion_size,
        multiUSVStatus_data,
        multiUSVStatus_size,
    )

    ##################################################################################
    # TODO: 在此处添加相关控制逻辑代码
    # 根据实时位置，任务信息求下一时刻艇的期望航速航向
    multiUSVInitInfo: List[USVInitStructType] = []
    for i, it in enumerate(multiUSVStatus_list):
        print(
            "ID: ",
            it.iUSVId,
            "X: ",
            it.dbCurX,
            "Y: ",
            it.dbCurY,
            "diastance：",
            math.sqrt(
                (it.dbCurX - Old_data[i][0]) ** 2 + (it.dbCurY - Old_data[i][1]) ** 2
            ),
            "Vel:",
            it.fCurVel - Old_data[i][2],
            "Yaw:",
            it.fCurYaw - Old_data[i][3],
        )
        Old_data[i][0] = it.dbCurX
        Old_data[i][1] = it.dbCurY
        Old_data[i][2] = it.fCurVel
        Old_data[i][3] = it.fCurYaw
    for it in multiUSVStatus_list:
        updateUSVControl = USVInitStructType()
        updateUSVControl.iUSVId = it.iUSVId
        updateUSVControl.dbInitX = it.dbCurX
        updateUSVControl.dbInitY = it.dbCurY
        updateUSVControl.fInitVel, updateUSVControl.fInitYaw = Go_to_xy(
            it.dbCurX, it.dbCurY, 0, 100
        )

        multiUSVInitInfo.append(updateUSVControl)
    return multiUSVInitInfo
    ##################################################################################


def Get_USV_data(
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    taskPath_list: List[SFormationTaskInfo] = [
        taskPath_data[index] for index in range(taskPath_size.value)
    ]
    referInfo_list: List[referUSVInfo] = [
        referInfo_data[index] for index in range(referInfo_size.value)
    ]
    curOBSFusion_list: List[InputOBSStruct] = [
        curOBSFusion_data[index] for index in range(curOBSFusion_size.value)
    ]
    multiUSVStatus_list: List[USVOutPutStructType] = [
        multiUSVStatus_data[index] for index in range(multiUSVStatus_size.value)
    ]
    Task_data = [] if len(taskPath_list) != 0 else Task_data
    Refe_data = [] if len(referInfo_list) != 0 else Refe_data
    Obs_data = [] if len(curOBSFusion_list) != 0 else Obs_data
    USV_data = [] if len(multiUSVStatus_list) != 0 else USV_data
    for it in taskPath_list:
        Task_data.append([it.referX, it.referY])
    for it in referInfo_list:
        Refe_data.append([it.relatDis, it.relatYaw, it.USVID, it.type])
    for it in curOBSFusion_list:
        # 这里的 obsR 和 fOBSV与说明书上反的
        Obs_data.append([it.dOBSX, it.dOBSY, it.fOBSV])
    for it in multiUSVStatus_list:
        USV_data.append([it.dbCurX, it.dbCurY])

    return multiUSVStatus_list


def plot_graph():
    global plotting
    global Task_data
    global Obs_data
    global USV_data
    global multiUSVInitInfo
    plt.ion()  # 开启交互模式
    while plotting:
        plt.clf()  # 清除之前的图表
        # 使用scatter绘制所有点
        # plt.scatter([x for x, _ in Task_data], [y for _, y in Task_data], color="blue")
        # for i, (x, y) in enumerate(Task_data):
        #     plt.text(x, y, f"Task{i}", color="blue", fontsize=8)
        #     circle = patches.Circle((x, y), 50, fill=False, edgecolor="blue")
        #     plt.gca().add_patch(circle)

        plt.scatter(
            [x for x, _, _ in Obs_data], [y for _, y, _ in Obs_data], color="green"
        )
        for i, (x, y, r) in enumerate(Obs_data):
            plt.text(x, y, f"Obs{i}", color="green", fontsize=8)
            circle = patches.Circle((x, y), r, fill=False, edgecolor="green")
            plt.gca().add_patch(circle)

        plt.scatter([x for x, _ in USV_data], [y for _, y in USV_data], color="red")
        for i, (x, y) in enumerate(USV_data):
            plt.text(x, y, f"USV{i}", color="red", fontsize=8)
            circle = patches.Circle((x, y), 50, fill=False, edgecolor="red")
            plt.gca().add_patch(circle)
        # 绘制箭头
        for i, updateUSVControl in enumerate(multiUSVInitInfo):
            dx = updateUSVControl.fInitVel * np.cos(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            dy = updateUSVControl.fInitVel * np.sin(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            plt.arrow(
                updateUSVControl.dbInitX,
                updateUSVControl.dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=20,
                fc="blue",
                ec="blue",
            )

        # 添加图例
        plt.legend()
        plt.title("示例图表")
        plt.axis("equal")  # 确保x和y轴的刻度一致，使得圆形看起来是圆的
        # plt.xlim(-300, 300)
        # plt.ylim(-300, 300)
        plt.draw()  # 重新绘制图表
        plt.pause(0.1)  # 暂停一段时间，等待图表更新
    plt.ioff()  # 关闭交互模式


if __name__ == "__main__":
    os.add_dll_directory(os.path.join(os.getcwd(), "./dll"))
    cmultilUSV = MultiUSVModel(
        dll_path=r"./dll/usvmodel.dll",
        dll_deps=[os.path.join(r"./dll", dll_i) for dll_i in os.listdir(r"./dll")],
    )
    # # 创建并启动绘图线程
    plot_thread = threading.Thread(target=plot_graph)
    plot_thread.start()

    # 态势信息
    curOBSFusion_data = ctypes.POINTER(InputOBSStruct)()  # 指向InputOBSStruct类型的指针
    curOBSFusion_size = ctypes.c_int()  # 障碍物数量
    # 导航信息
    multiUSVStatus_data = ctypes.POINTER(USVOutPutStructType)()
    multiUSVStatus_size = ctypes.c_int()

    isEnd_int = ctypes.c_int()
    # 任务航路
    taskPath_data = ctypes.POINTER(SFormationTaskInfo)()
    taskPath_size = ctypes.c_int()
    # 构型信息
    referInfo_data = ctypes.POINTER(referUSVInfo)()
    referInfo_size = ctypes.c_int()
    # 整个任务迭代次数
    iterNum = 0
    # 控制模式，1 - 航速航向；2 - 航路，建议使用航速航向
    flagModel = 1
    # 算法更新后清零
    algIterNum = 0

    # 初始化
    cmultilUSV.multiInitialize(flagModel)
    try:
        for i in range(100):
            # 更新控制指令
            cmultilUSV.updateMultiUSVControl(
                (USVInitStructType * len(multiUSVInitInfo))(*multiUSVInitInfo),
                len(multiUSVInitInfo),
                iterNum,
                flagModel,
            )
            # 获取导航信息和态势信息
            cmultilUSV.getMultiNaviObsInfo(
                iterNum,
                ctypes.byref(multiUSVStatus_data),
                ctypes.byref(multiUSVStatus_size),
                ctypes.byref(curOBSFusion_data),
                ctypes.byref(curOBSFusion_size),
            )
            # 获取任务信息
            rcvFlag = cmultilUSV.getTaskInfo(
                ctypes.byref(isEnd_int),
                ctypes.byref(taskPath_data),
                ctypes.byref(taskPath_size),
                ctypes.byref(referInfo_data),
                ctypes.byref(referInfo_size),
                iterNum,
            )
            # 触发任务
            if 1 == rcvFlag:
                if 1 == isEnd_int.value:  # 到达最后一个任务点，不再发送任务
                    cmultilUSV.taskEnd(iterNum, flagModel)
                    break  # 跳出算法循环
                algIterNum = 0  # 新任务，次数清零
            # 算法更新控制指令
            multiUSVInitInfo: List[USVInitStructType] = algorithmImpl(
                algIterNum,
                taskPath_data,
                taskPath_size,
                referInfo_data,
                referInfo_size,
                curOBSFusion_data,
                curOBSFusion_size,
                multiUSVStatus_data,
                multiUSVStatus_size,
            )

            # 迭代次数更新
            iterNum += 1
            algIterNum += 1
        # 当主线程的任务完成后，更新标志以停止绘图线程的循环
    except KeyboardInterrupt:
        plotting = False
        # 等待绘图线程结束
        plot_thread.join()
